/*
 *  CollisionSystem.cpp
 *  GLRender
 *
 *  Created by Patrick zulian on 5/18/10.
 *  Copyright 2010 University  of Lugano. All rights reserved.
 *
 */

#include "CollisionSystem.h"
#include "WRenderer.h"

void 
WNode::init()
{
	for (int i = 0; i < 8; i++) {
		children[i] = NULL;
	}

}


WNode::WNode(const Point3d &min, const Point3d &max)
: min(min), max(max), hasChildren(false), maxCapacity(10), level(0), refCount(0), parent(NULL), root(NULL)
{
	
}

WNode::WNode(): hasChildren(false), maxCapacity(10), level(0), refCount(0), parent(NULL), root(NULL) {  }
	

WNode::~WNode()
{
	if (hasChildren) {
		for(int i = 0; i < 8; ++i) {
			delete children[i];
		}
	}
}

void
WNode::deleteChildren()
{
	if (hasChildren) {
		for(int i = 0; i < 8; ++i) {
			collSys->leafRemoved(children[i]);
			delete children[i];
		}
	}
	hasChildren = false;
	collSys->leafAdded(this);
}

Point3d 
WNode::getSubdivisionIndex(const int i) const
{
	const int D = 3;
	Point3d index;
	int remainder, p, sub;
	remainder = i;
	for(int d = 0; d < D; d++) {
		p = utils::tpow(2, D - (d+1));
		sub = remainder - p;
		if (sub >= 0) {
			index[d] = 1;
			remainder = sub;
		} else {
			index[d] = 0;
		}
		
	}
	return index;
}


void
WNode::newSubCellBound(const int subcellNumber, Point3d * cmin, Point3d * cmax) const
{
	

	Point3d subcellIndex = getSubdivisionIndex(subcellNumber);
	
	for(int d = 0; d < 3; d++) {
		const double sizeD =  fabs((max[d] - min[d])/2.0);
		(*cmin)[d] =  min[d] + subcellIndex[d] * sizeD;	
		(*cmax)[d] =  min[d] + ( subcellIndex[d] + 1.0 ) * sizeD;
	}
}
	
void
WNode::initChildren()
{
	
	collSys->leafRemoved(this);
	
	for (int i = 0; i < 8; ++i) {
		Point3d cmin, cmax;
		newSubCellBound(i, &cmin, &cmax);
		children[i] = new WNode(cmin, cmax);
		assert( children[i] != NULL );
		children[i]->level = level + 1;
		children[i]->parent = this;
		children[i]->parent = getRoot();
		collSys->leafAdded(children[i]);
	}
	
	hasChildren = true;	
}

bool 
WNode::insert(WObject * object)
{
	if (!isIn(object)) {
		return false;
	}

	if (parent != NULL) {
		parent->refCount++;
	}
	
	if (!hasChildren) {
		objects.push_back(object);
		//object->addListener(this);
		
		if(objects.size() <= maxCapacity) {
			return true;
		}
		 else {
			initChildren();
			pushDown();
			return true;
		}
	}
	
	bool inside = false;
	for (int i = 0; i < 8; ++i) {
		if (children[i]->insert(object)) {
			inside = true;
		}
	}
	
	assert(inside);
	return inside;
}

void 
WNode::pushDown() 
{
	while (!objects.empty()) {
		WObject * object = objects.back();
		//object->removeListener(this);
		bool inside = false;
		for (int i = 0; i < 8; ++i) {
			if(children[i]->insert(object)) {
				inside = true;
			}
		}
		assert(inside);
		objects.pop_back();
	
	}
	assert(objects.empty());
}

bool 
WNode::isIn(WObject * object)
{
	const double disp = object->getRadius() + TOL;
	Point3d objPos = object->position();
	
	for (int d = 0; d < 3; d++) {
		const double objMin = std::min< double >( objPos[d] + disp, objPos[d] - disp );
		const double objMax = std::max< double >( objPos[d] + disp, objPos[d] - disp );
		const bool inside = ( objMin >= min[d] && objMin <= max[d] ) || ( objMax <= max[d] && objMax >= min[d] );
		
		if (!inside) {
			return false;
		}
	}

	return true;
}

bool 
WNode::remove(WObject * object)
{
	if (!isIn(object)) {
		return false;
	}
	
	bool removed = false;
	
	if (!objects.empty()) {
		
		std::vector< WObject * >::iterator iter = objects.begin(); 
		while( iter != objects.end() ) {
			if ((*iter) == object) {
				assert(!removed);
				iter = objects.erase(iter);
				//object->removeListener(this);
				removed = true;
			}
			
			else {
				++iter;
			}
		}
	}
	
	if (hasChildren) {
		for (int i = 0; i < 8; ++i) {
			if (children[i]->remove(object)) {
				removed =  true;
			}
		}
	}
	
	
	if (removed) {
		if( refCount == 0 ) deleteChildren();
		if( parent != NULL ) parent->refCount--;
	}
	
	return removed;
}


bool 
WNode::checkCollisions()
{
	if (objects.empty()) return false;
	
	bool collided = false;
	
	
	for (unsigned int i = 0; i < objects.size(); ++i) {
		for (unsigned int j = i+1; j < objects.size(); ++j) {		
			if (objects[i] != objects[j] && areColliding(objects[i], objects[j])) {
				objects[i]->onCollision(objects[j]);
				objects[j]->onCollision(objects[i]);
				collided = true;
			}
		}
	}
	
	return collided;
}


bool 
WNode::checkCollision(WObject * object)
{
	if (!isIn(object)) {
		return false;
	}
	
	
	if (objects.empty()) goto CHILDREN;
	
	
	for (std::vector< WObject * >::iterator it = objects.begin(); it != objects.end(); ++it) {
		assert(object != *it);
		if (object != *it && areColliding(object, *it)) {
			object->onCollision(*it);
			(*it)->onCollision(object);
			return true;
		}
	}
	
CHILDREN:
	if (!hasChildren) {
		return false;
	}
	
	for (int i = 0; i < 8; ++i) {
		if(children[i]->checkCollision(object)) return true;
	}
	
	
	return false;
}

bool 
WNode::areColliding(WObject * obj1, WObject * obj2)
{
	const double maxDistance = obj1->getRadius() + obj2->getRadius();
	const double distance = (obj1->position() - obj2->position()).norm();
	return maxDistance > distance;
}


bool 
WNode::directRemove(WObject * object)
{
	if (objects.empty()) return false;
	bool removed = false;
	
	std::vector< WObject * >::iterator iter = objects.begin(); 
	while( iter != objects.end() ) {
		if ((*iter) == object) {
			assert(!removed); //only once
			iter = objects.erase(iter);
			//object->removeListener(this);
			removed = true;
		}
		
		else {
			++iter;
		}
	}
	
	return removed;
}






void 
WNode::clear()
{	
	objects.clear();
	if (hasChildren) {
		for(int i = 0; i < 8; ++i) {
			delete children[i];
		}
	}
	hasChildren = false;
	refCount = 0;
}

void 
WNode::reset(const Point3d &min, const Point3d &max)
{
	clear();
	this->min = min; 
	this->max = max;
}


void 
WNode::objectMoved( WObject * object, const int callbackNumber )
{
	if(!object->alive()) return;
	if ( !completelyIn(object) /*|| !isIn(object)*/ ) {
		const bool removed = directRemove(object);
		assert(removed);
		if (callbackNumber == 0) {
			pushUp(object);
		}
	}	
}



bool 
WNode::completelyIn(WObject * object)
{
	const double disp = object->getRadius() + TOL;
	Point3d objPos = object->position();
	
	for (int d = 0; d < 3; d++) {
		const double objMin = std::min< double >( objPos[d] + disp, objPos[d] - disp );
		const double objMax = std::max< double >( objPos[d] + disp, objPos[d] - disp );
		const bool inside = ( objMin >= min[d] && objMax <= max[d] );
		if (!inside) {
			return false;
		}
	}
	
	return true;

}


bool 
WNode::pushUp(WObject * object)
{

	WNode * anchestor = parent;
	
	while (anchestor != NULL) {
		if (anchestor->completelyIn(object)) {
			assert(anchestor->insert(object));
			return true;
		}
		anchestor = anchestor->parent;
	}
	
	return false;
}



void
WNode::renderOn(AbstractRenderer * renderer)
{
	
	if (objects.empty() && !hasChildren) {
		return;
	}

	if (level == 0) glDisable(GL_LIGHTING);
	
	glLineWidth(level + 1.0f);
	renderer->renderWNode(this);
	
	
	for (std::vector< WObject * >::iterator it = objects.begin(); it != objects.end(); ++it) {
		glPushMatrix();
		
		const Point3d center = (*it)->position();
		const double radius = (*it)->getRadius();

		glColor3f(1.0, 0.0, (float) (*it)->alive());
		glTranslated(center[0], center[1], center[2]);
		glScaled(radius, radius, radius);
		
		((WRenderer *)renderer)->renderCross();
		
		glPopMatrix();
		
	}
	
	
	
	if(hasChildren) {
		for (int i = 0; i < 8; ++i) {
			children[i]->renderOn(renderer);
		}
	}
	
	if (level == 0) glEnable(GL_LIGHTING);
}




bool 
CollisionSystem::checkCollisions()
{
	bool collided = false;
	LeavesIt it;
	
	for (it = leaves.begin(); it!= leaves.end(); ++it) {
		collided |=(*it)->checkCollisions();
		
	}
	return collided;
}

